#include <stereo_api.h>

using namespace cv;
using namespace std;

int main()
{
#if 0
	//front parameters
	Mat leftIntrinsics = (Mat_<double>(3,3) << 482.944092, 0., 320.877441,
												0., 482.944092, 239.427063,
												0., 0., 1.0);

	Mat rightIntrinsics = (Mat_<double>(3,3) << 482.944092, 0., 320.877441,
												0., 482.944092, 239.427063,
												0., 0., 1.0);

	Mat rotationLeftInRight = (Mat_<double>(3,3) << 1.0, 0., 0.,
													0., 1.0, 0.,
													0., 0., 1.0);

	Mat translationLeftInRight = (Mat_<double>(3,1) << -99.373116, 0., 0.);
#else
	//down parameters
	Mat leftIntrinsics = (Mat_<double>(3,3) << 481.071198, 0., 319.511902,
												0., 481.071198, 237.524200,
												0., 0., 1.0);

	Mat rightIntrinsics = (Mat_<double>(3,3) << 481.071198, 0., 319.511902,
												0., 481.071198, 237.524200,
												0., 0., 1.0);

	Mat rotationLeftInRight = (Mat_<double>(3,3) << 1.0, 0., 0.,
													0., 1.0, 0.,
													0., 0., 1.0);

	Mat translationLeftInRight = (Mat_<double>(3,1) << -79.615486, 0., 0.0);
#endif

	Mat discoff = (Mat_<double>(5,1) << 0., 0., 0., 0., 0.);

#if 0
	Mat leftImage = imread("/home/wenyi/workspace/stereoImage/left_front.jpg", IMREAD_GRAYSCALE);
	Mat rightImage = imread("/home/wenyi/workspace/stereoImage/right_front.jpg", IMREAD_GRAYSCALE);
#else
	Mat leftImage = imread("/home/wenyi/workspace/stereoImage/left_down.jpg", IMREAD_GRAYSCALE);
	Mat rightImage = imread("/home/wenyi/workspace/stereoImage/right_down.jpg", IMREAD_GRAYSCALE);
#endif

	Mat leftDisp, xyz;

	stereoCalculateDepthMap(leftImage, rightImage, leftIntrinsics, rightIntrinsics, discoff, discoff, rotationLeftInRight,translationLeftInRight, leftDisp, xyz);

	imwrite("./testImage/leftDisp.jpeg", leftDisp);

	Mat zImage(xyz.rows, xyz.cols, CV_8UC1);
	//printf every point 3d coordinate
	for(int y = 0; y < xyz.rows; y++)
    {
        for(int x = 0; x < xyz.cols; x++)
        {
            Vec3f point = xyz.at<Vec3f>(y, x);
			float distance = sqrt(pow(point[0], 2) + pow(point[1], 2) + pow(point[2], 2));
			if( 0.7f < distance && distance < 40.0f)
			{
				zImage.at<uchar>(y,x) = floor(point[2]);
				printf("x:%f, y: %f, z: %f, distance:%f m\n", point[0], point[1], point[2], sqrt(pow(point[0], 2) + pow(point[1], 2) + pow(point[2], 2)));
			}
			else
			{
				zImage.at<uchar>(y,x) = 255;
			}
        }
    }

	imwrite("./testImage/zImage.jpeg", zImage);

	Mat dispTest;
	int numberOfDisparities = ((leftImage.size().width/8) + 15) & -16;
	leftDisp.convertTo(dispTest, CV_8U, 255/(numberOfDisparities*16.));

	imwrite("./testImage/dispTest.jpeg", dispTest);
	return 0;
}
